#if defined(__EXCEPTIONS)
#undef __EXCEPTIONS
#endif
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <iostream>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) 
{
    Eigen::Quaterniond q_1(0.2, 0.3, 0.1, 0.35);
    Eigen::Quaterniond q_2(0.4, -0.1, 0.2, -0.5);
    q_1.normalize();
    q_2.normalize();
    Eigen::Vector3d t_1(0.3, 0.1, 0.1);
    Eigen::Vector3d t_2(-0.1, 0.5, 0.3);
    Eigen::Vector3d p_1(0.5, 0, 0.2);
    Eigen::Vector3d p_2;
    Eigen::Matrix3d R_1 = q_1.matrix();
    Eigen::Matrix3d R_2 = q_2.matrix();
    Eigen::Isometry3d T_1 = Eigen::Isometry3d::Identity();
    Eigen::Isometry3d T_2 = Eigen::Isometry3d::Identity();
    T_1.rotate(R_1);
    T_2.rotate(R_2);
    T_1.pretranslate(t_1);
    T_2.pretranslate(t_2);

    p_2 = T_2 * T_1.inverse() * p_1;
    cout.precision(2);
    cout << p_2.transpose() << endl;

    // Isometry3d 是 4x4矩阵
    Eigen::Isometry3d T_1w(q_1), T_2w(q_2);
    cout << "befor : \n" << T_1w.matrix() << endl;
    T_1w.pretranslate(t_1);
    cout << "after : \n" << T_1w.matrix() << endl;
    T_2w.pretranslate(t_2);
    p_2 = T_2w * T_1w.inverse() * p_1;
    cout << p_2.transpose() << endl;

    return 0;
}
